#include <stdlib.h>
#include <stdio.h>
#include "fliter.h"

#define SIN sin
#define TAN tan

BWLowPass* create_bw_low_pass_filter(int order, FTR_PRECISION s, FTR_PRECISION f) {
    FTR_PRECISION a;
    FTR_PRECISION a2;
    FTR_PRECISION r;
    int i = 0;

    BWLowPass* filter = (BWLowPass*)malloc(sizeof(BWLowPass));
    if (!filter) return NULL;

    filter->n = order / 2;
    filter->A = (FTR_PRECISION*)malloc(filter->n * sizeof(FTR_PRECISION));
    filter->d1 = (FTR_PRECISION*)malloc(filter->n * sizeof(FTR_PRECISION));
    filter->d2 = (FTR_PRECISION*)malloc(filter->n * sizeof(FTR_PRECISION));
    filter->w0 = (FTR_PRECISION*)calloc(filter->n, sizeof(FTR_PRECISION));
    filter->w1 = (FTR_PRECISION*)calloc(filter->n, sizeof(FTR_PRECISION));
    filter->w2 = (FTR_PRECISION*)calloc(filter->n, sizeof(FTR_PRECISION));

    if (!filter->A || !filter->d1 || !filter->d2 || !filter->w0 || !filter->w1 || !filter->w2) {
        free(filter->A);
        free(filter->d1);
        free(filter->d2);
        free(filter->w0);
        free(filter->w1);
        free(filter->w2);
        free(filter);
        return NULL;
    }

    a = TAN(M_PI * f / s);
    a2 = a * a;

    for (i = 0; i < filter->n; ++i) {
        r = SIN(M_PI * (2.0 * i + 1.0) / (4.0 * filter->n));
        s = a2 + 2.0 * a * r + 1.0;
        filter->A[i] = a2 / s;
        filter->d1[i] = 2.0 * (1 - a2) / s;
        filter->d2[i] = -(a2 - 2.0 * a * r + 1.0) / s;
    }
    return filter;
}

void free_bw_low_pass(BWLowPass* filter) {
    if (filter) {
        free(filter->A);
        free(filter->d1);
        free(filter->d2);
        free(filter->w0);
        free(filter->w1);
        free(filter->w2);
        free(filter);
    }
}
